﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using Drifting.MotionPlanning.SingleThreading.Algorithms;
using Drifting.MotionPlanning.Primitives;
using Drifting.Graphics.EffectObjects;
using Drifting.Foundation;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework;

namespace Drifting.MotionPlanning.SingleThreading.Planners
{
    public class Planner
    {
        TreeBasedPlanningAlgorithm algorithm;
        Thread planningThread;
        Thread vertexTranslationThread;

        protected Skin_G skin_vertices;
        List<State> skin_v1;
        List<State> skin_v2;

        double _measurement_averagedCost;
        

        public Planner(TreeBasedPlanningAlgorithm algorithm, GraphicsDevice device, GoalCheck goalChecker)
        {
            this.algorithm = algorithm;

            planningThread = new Thread(algorithm.Execute);
            planningThread.Priority = ThreadPriority.Highest;

            vertexTranslationThread = new Thread(VisualizeVertices);
            vertexTranslationThread.Priority = ThreadPriority.Lowest;

            skin_v1 = new List<State>(10240);
            skin_v2 = new List<State>(10240);
            skin_vertices = new Skin_G(skin_v1, device);

            algorithm.SetGoalChecker(goalChecker);

            _measurement_averagedCost = 0;
        }

        public Planner(TreeBasedPlanningAlgorithm algorithm, GraphicsDevice device, Skin_G visualizer, GoalCheck goalChecker)
        {
            this.algorithm = algorithm;

            planningThread = new Thread(algorithm.Execute);
            planningThread.Priority = ThreadPriority.Highest;

            vertexTranslationThread = new Thread(VisualizeVertices);
            vertexTranslationThread.Priority = ThreadPriority.Lowest;

            skin_v1 = new List<State>(10240);
            skin_v2 = new List<State>(10240);
            skin_vertices = visualizer;
            visualizer.UpdateDataContent(skin_v1);

            algorithm.SetGoalChecker(goalChecker);
        }


        public Skin_G Skin
        {
            get { return skin_vertices; }
        }

        public void VisualizeVertices()
        {
            while (true)
            {
                List<State> backList = skin_v1;
                if (skin_vertices.isBeingRendered(backList))
                    backList = skin_v2;

                algorithm.Vertices.ReportVerticesSnapshot(ref backList);
                skin_vertices.UpdateDataContent(backList);

                double _measurement_averagedCost_new = 0;
                int n_values = 0;
                foreach (State x in backList)
                {
                    if (x == null) continue;
                    _measurement_averagedCost_new += x.Cost;
                    n_values++;
                }
                _measurement_averagedCost_new /= n_values;
                _measurement_averagedCost = _measurement_averagedCost_new;

                Thread.Sleep(100);
            }
        }

        public void Translate_StateSpace_into_GraphicalSpace(ref List<float[]> dataList)
        {
            foreach (float[] q in dataList)
            {
                
            }
        }

        public TreeBasedPlanningAlgorithm Algorithm
        {
            get { return algorithm; }
        }

        public bool Viewable
        {
            get
            {
                if (vertexTranslationThread.ThreadState == ThreadState.Running)
                    return true;
                else
                    return false;
            }
            set
            {
                if (value == true)
                {
                    vertexTranslationThread.Start();
                }
                else
                {
                    if(vertexTranslationThread.ThreadState == ThreadState.Running)
                        vertexTranslationThread.Join();
                }
            }
        }

        public void Start(int iterations)
        {
            planningThread.Start(iterations);
        }

        public void Terminate()
        {
            vertexTranslationThread.Abort();
            planningThread.Abort();
        }

        public void GetStatistics(ref List<string> info)
        {
            if (info == null)
                info = new List<string>(1);
            
            while (info.Count < 1)
                info.Add(null);

            algorithm.ReportStatistics(ref info);

            while (info.Count < 3)
                info.Add(null);
            info[2] = "average cost = " + _measurement_averagedCost;         
        }
        
    }
}
